#include "alignedface.h"
#include <random>
#ifdef DEBUG_ALIGNEDFACE
#include "facedetect.h"
#endif

#define USE_ESTIMATEAFFINE2D 1
// in 192 192
#if USE_ESTIMATEAFFINE2D
std::vector<Point2f> AlignedFace::ref5 = {
#if 1
    Point2f (57.513999938964844, 66.02166748046875), // left eye
    Point2f (138.21258544921875, 65.71470642089844), // right eye
    Point2f (98.03968811035156, 105.46015167236328), // nose
    Point2f (68.94014739990234, 130.07565307617188), // left mouth
    Point2f (127.49958038330078, 129.8214569091797), // right mouth
#else
    Point2f (68.19915771484375, 98.91389465332031),   // left eye
    Point2f (127.57298278808594, 98.65808868408203),  // right eye
    Point2f (91.47057342529297, 125.21678924560547),  // nose
    Point2f (72.47095489501953, 148.35472106933594),  // left mouth
    Point2f (119.95799255371094, 148.14288330078125), // right mouth

// Point2f(70.82415771484375,88.41389465332031),   // left eye
// Point2f(124.94798278808594,88.15808868408203),   // right eye
// Point2f(94.09557342529297,110.77928924560547),   // nose
// Point2f(75.09595489501953,137.06723022460938),   // left mouth
// Point2f(121.27049255371094,136.8553924560547),   // right mouth
#endif
};
#else
std::vector<Point2f> AlignedFace::ref5 = {
    Point2f (68.19915771484375, 98.91389465332031),  // left eye
    Point2f (127.57298278808594, 98.65808868408203), // right eye
    Point2f (91.47057342529297, 125.21678924560547), // nose
};
#endif
AlignedFace::AlignedFace (std::string id) {
    name = id;
}

AlignedFace::~AlignedFace () {}
// 计算仿射矩阵的反变换矩阵
Mat AlignedFace::computeAffineInverse (const Mat &affine) {
    // 分解仿射矩阵：线性部分A（2x2）和平移部分t（2x1）
    Mat A = affine (cv::Rect (0, 0, 2, 2)); // 前两列，前两行
    Mat t = affine (cv::Rect (2, 0, 1, 2)); // 第三列，前两行

    // 计算线性部分的逆矩阵A_inv（需确保A非奇异）
    Mat   A_inv;
    float det = cv::determinant (A); // 计算行列式（判断是否可逆）
    if (std::abs (det) < 1e-6) {
        throw std::runtime_error ("仿射矩阵的线性部分不可逆（行列式接近0）");
    }
    invert (A, A_inv);

    // 计算反变换的平移部分：t_inv = -A_inv * t
    Mat t_inv = -A_inv * t;

    // 组合反变换矩阵：[A_inv | t_inv]（2x3）
    Mat inverse_affine;
    hconcat (A_inv, t_inv, inverse_affine);
    return inverse_affine;
}

void AlignedFace::Run (const Args &input,Args& output) {
    const float *f = input.args.data ();
#if USE_ESTIMATEAFFINE2D
    std::vector<Point2f> src = {
        {f[4],  f[5] },
        {f[6],  f[7] },
        {f[8],  f[9] },
        {f[10], f[11]},
        {f[12], f[13]}
    };
#else
    std::vector<Point2f> src = {
        {f[4], f[5]},
        {f[6], f[7]},
        {f[8], f[9]}
    };
#endif
    try {
        std::cout << "src: " << src << std::endl;
        std::cout << "ref5: " << ref5 << std::endl;

#if USE_ESTIMATEAFFINE2D
        std::vector<uchar> inliers;
        Mat                est_affine = cv::estimateAffine2D (src, // 源点
                           ref5,                                   // 目标点
                           inliers,                                // 输出内点标记
                           cv::RANSAC, // 鲁棒估计方法（可选：0=简单L2，RANSAC=随机抽样一致性）
                           3.0,        // RANSAC重投影阈值（像素误差超过此值视为外点）
                           2000,       // RANSAC最大迭代次数
                           0.9995      // RANSAC期望置信度
                       );
        // Mat est_affine;
        // if (!estimateAffine2D (src, ref5, est_affine, inliers)) {
        //     return output;
        // }
#else
        Mat est_affine = getAffineTransform (src, ref5);
#endif
        std::cout << "est_affine: " << est_affine << std::endl;

        if (est_affine.empty ())
            return;
        warpAffine (input.image, output.image, est_affine, Size (192, 192), cv::INTER_LINEAR, cv::BORDER_CONSTANT,
            cv::Scalar (255, 255, 255));
        Mat                 inverse_affine = computeAffineInverse (est_affine);
        std::vector<float> &vec = output.args;

        for (int i = 0; i < inverse_affine.rows; ++i) {
            for (int j = 0; j < inverse_affine.cols; j++) {
                vec.push_back (inverse_affine.ptr<double> (i)[j]);
            }
        }
    } catch (...) {
        std::cerr << "alignedface:计算错误" << std::endl;
    }
}

// 使用RANSAC算法估计2D仿射变换（无显式内存分配）
bool AlignedFace::estimateAffine2D (const std::vector<cv::Point2f> &srcPoints,
    const std::vector<cv::Point2f> &dstPoints, cv::Mat &affineTransform, std::vector<uchar> &inliers,
    float reprojectionThreshold, int maxIters, float confidence) {
    if (srcPoints.size () < 3 || srcPoints.size () != dstPoints.size ()) {
        return false;
    }

    // 初始化随机数生成器
    std::srand (static_cast<unsigned int> (std::time (nullptr)));

    // 最佳模型参数
    cv::Mat            bestTransform = cv::Mat::eye (2, 3, CV_32F);
    int                bestInlierCount = 0;
    std::vector<uchar> bestInliers (srcPoints.size (), 0);

    // RANSAC迭代
    for (int iter = 0; iter < maxIters; iter++) {
        // 1. 随机选择3个不重复的点
        std::vector<int> indices;
        while (indices.size () < 3) {
            int idx = std::rand () % srcPoints.size ();

            // 确保不重复
            if (std::find (indices.begin (), indices.end (), idx) == indices.end ()) {
                indices.push_back (idx);
            }
        }
        // 2. 提取随机选择的点
        std::vector<cv::Point2f> srcSample (3);
        std::vector<cv::Point2f> dstSample (3);
        for (int i = 0; i < 3; i++) {
            srcSample[i] = srcPoints[indices[i]];
            dstSample[i] = dstPoints[indices[i]];
        }
        // 3. 计算仿射变换
        cv::Mat currentTransform = cv::getAffineTransform (srcSample, dstSample);
        if (currentTransform.empty ()) {
            continue; // 点可能共线，跳过
        }

        // 4. 计算内点数量
        int                currentInlierCount = 0;
        std::vector<uchar> currentInliers (srcPoints.size (), 0);
        for (size_t i = 0; i < srcPoints.size (); i++) {
            // 应用变换
            cv::Point2f transformed;
            transformed.x = currentTransform.at<double> (0, 0) * srcPoints[i].x
                            + currentTransform.at<double> (0, 1) * srcPoints[i].y + currentTransform.at<double> (0, 2);
            transformed.y = currentTransform.at<double> (1, 0) * srcPoints[i].x
                            + currentTransform.at<double> (1, 1) * srcPoints[i].y + currentTransform.at<double> (1, 2);
            // 计算误差
            float dx = transformed.x - dstPoints[i].x;
            float dy = transformed.y - dstPoints[i].y;
            float error = std::sqrt (dx * dx + dy * dy);

            // 判断是否为内点
            if (error < reprojectionThreshold) {
                currentInliers[i] = 1;
                currentInlierCount++;
            }
        }
        // 5. 更新最佳模型
        if (currentInlierCount > bestInlierCount) {
            bestInlierCount = currentInlierCount;
            currentTransform.copyTo (bestTransform);
            bestInliers = currentInliers;

            // 提前终止条件
            if (bestInlierCount >= srcPoints.size ())
                break;

            // 更新迭代次数
            float pNoOutliers = std::pow ((float)(srcPoints.size () - bestInlierCount) / srcPoints.size (), 3);
            float eps = 1e-6f;
            if (1.0f - pNoOutliers < eps)
                pNoOutliers = 1.0f - eps;
            maxIters = std::min (maxIters, (int)std::ceil (std::log (1.0f - confidence) / std::log (pNoOutliers)));
        }
    }

    // 复制结果
    bestTransform.copyTo (affineTransform);
    inliers = bestInliers;

    return bestInlierCount >= 3;
}

#ifdef DEBUG_ALIGNEDFACE
int main (int argc, char *argv[]) {
    FaceDectect faces;
    Mat         image = imread ("test_face.jpg");
    if (image.empty ()) {
        std::cerr << "无法加载图像" << std::endl;
        return -1;
    }
    faces.Run (image);
    Mat detFaces = faces.getResult ();
    if (!detFaces.empty ()) {
        for (int i = 0; i < detFaces.rows; i++) {
            AlignedFace alignedFace;
            Args        in;
            in.image = image;
            std::vector<float> &vec = in.args;
            vec.insert (vec.end (), detFaces.ptr<float> (i), detFaces.ptr<float> (i) + detFaces.cols);
            Args out = alignedFace.Run (in);
            if (!out.image.empty ()) {
                char s[100];
                snprintf (s, 100, "out%d.jpg", i);
                imwrite (s, out.image);
            }
        }
    }

    return 0;
}
#endif
// g++ -std=c++11 alignedface.cpp facedetect.cpp -o alignedface -L/home/android/opencv/build/install/usr/local/lib
// `pkg-config --cflags --libs opencv4` -DDEBUG_ALIGNEDFACE=1
// LD_LIBRARY_PATH=/home/android/opencv/build/install/usr/local/lib
